

s = tf('s');
testt = (2.1)/(0.1*s^2 + 0.2*s +1); %transfer function

stepsize=0.007;
tend=3; 
ts=(0:stepsize:tend)'; %sampling times
N=size(ts);
N=N(1);


omega=5; 
testinp=@(t) sin(omega*t.*t); %input function
u=testinp(ts); %inputs at times

degdiffu=2;
dtestinp={@(t) sin(omega*t.*t),@(t) 2*t*omega.*cos(omega*t.*t), @(t) 2*omega*cos(omega*t.*t)-4*t.*t*omega*omega.*sin(omega*t.*t)};

du=zeros(N,degdiffu+1); %structfun(@(f)f(ts), dtestinp, 'UniformOutput', false);
for k=1:degdiffu+1
    du(:,k)=dtestinp{k}(ts);
end

%du=zeros(N,degdiffu);
%for j=1:degdiffu
%    tt=dtestinp(j);
%    du(:,j)=tt(ts);
%end


yp=lsim(testt,u,ts); %y_pure

disturbance=random('norm',0,ones(size(ts))*0.02);

%später noch durch transfer function jagen??

y=yp+disturbance;




%ft=fittype('poly6');


%fitp=480;

degfitp=6;
degdiffy=2;
nfitp=80;

dy = zeros(N,degdiffy+1);
dy(:,1) = y; %möglicherweise noch smoothen


for fitp=(nfitp/2 + 2):(N - nfitp/2 - 2)
    inds=((fitp-nfitp/2):(fitp+nfitp/2));
    fo=fit(ts(inds),dy(inds,1),'poly6');
    dy(fitp,2:3)=differentiate(fo,ts(fitp)); %!!!! hardcoded stuff
end


S=[-dy(:,2:3),du];

phat=S\y;



%polv=feval(fo,ts).*[zeros(470,1);ones(60,1);zeros(471,1)];





%excludedata(ts,y,'domain',[j-nfitp/2,j+nfitp/2]);




%pol=polyfit(ts(inds),y(inds),degfitp);

%[pol,s,mu]=polyfit(ts(inds),y(inds),degfitp);

%polv=polyval(pol,(ts-mu(1))/mu(2)).*[zeros(470,1);ones(60,1);zeros(471,1)];


polv2=polyval(polyfit(ts(inds),y(inds),degfitp),ts).*[zeros(470,1);ones(60,1);zeros(471,1)];